/********** Local Includes ****************************************************/

#include "voronoi.h"
#include "objectdetector.h"
#include "common.h"
#include "imagesource.h"
#include "robot_orientation.h"

/********** Macros and Constants **********************************************/

#define BLUE cvScalar(255, 0, 0)
#define GREEN cvScalar(0, 255, 0)

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Local Function Prototypes *****************************************/

/**
 * Matlab-style all() function.
 *
 * Returns true if all of the values in the matrix are non-zero, or false
 * otherwise.
 *
 * The given matrix should be type CV_32FC1.
 */
bool nonZero(const Mat &);

/**
 * Flips the 1's and 0's of a binary matrix
 *
 * Returns the flipped matrix
 *
 */
Mat flip( Mat matrix);

/********** Public Methods ****************************************************/

Voronoi::Voronoi(ImageSource *source, Mat open) :
m_source(source),
m_open(open)
{
	if (!m_source) {
		ERROR("Image source cannot be null");
	}
	
	
}


Mat Voronoi::brushFire() {
    
    int sample = 15;
    
    Size gridSize = m_open.size();
    int rows =gridSize.height/blockSize();
    int cols = gridSize.width/blockSize();
     cout << "Grid has " << rows<< " rows and "<< cols<< "cols."<< endl;
    
    Mat grid(Size(cols, rows), CV_32FC1);
    grid = 1;
    
    Mat temp;
    cvtColor(m_open, temp, CV_GRAY2BGR);

    //in m_open 1 means pixel is not changed(no obstacle)
    //in grid 0 means blocked
   
    Mat test = flip(m_open.clone());
     //imshow("flipped", test);
    Size testSize = test.size();
    
    
       cout <<"first print" << endl;
    for ( int row = 0; row < sample; ++ row )
    {
        for ( int col = 0; col < sample; ++ col )
        {
            cout << test.at<float>(col, row) << "  ";
            
        }
        cout << endl;
    }
        cout <<" " << endl;

    int count = 2;
    
     while(!nonZero(test)){     
    
        //left
    //while(count <600){
         
        for ( int row = 0; row < testSize.width; ++ row )
        {
            for ( int col = 0; col < testSize.height; ++ col ){
                
                // cout <<"checking row "<< row<< "and col "<< col << endl;
                //left
                //if(col>0){
                    if ((test.at<float>(col, row) ==0) &&
                       ((test.at<float>(col+1, row) != 0) &&
                        (test.at<float>(col+1, row) <count) )) {
                           
                            test.at<float>(col, row) =count;
                    }
             //   }
                //right
                if(col > 0 && col < testSize.height){
                if ((test.at<float>(col, row) ==0) &&
                    ((test.at<float>(col-1, row) != 0) &&
                     (test.at<float>(col-1, row) <count) )) {
                        
                        test.at<float>(col, row) =count;
                    }
                }
                //above
               // if(row >0){
                    if ((test.at<float>(col, row) ==0) &&
                    ((test.at<float>(col, row+1) != 0) &&
                     (test.at<float>(col, row+1) <count) )) {
                        
                        test.at<float>(col, row) =count;
                    }
             //   }
                //below
                if(row > 0 && row < testSize.width){
                   if ((test.at<float>(col, row) ==0) &&
                    ((test.at<float>(col, row-1) != 0) &&
                     (test.at<float>(col, row-1) <count) ))
                   {
                        
                        test.at<float>(col, row) =count;
                   }
                }
            }
            
        }
         cout <<"checking level  "<< count << endl;
        count++; 
    }
   //}
    
    imshow("gradient", test); 
    cout <<"bottom left corner "<<  test.at<float>(400, 610) << endl;
    
	
    //print segment of matrix
    for ( int row = 0; row < sample; ++ row )
    {
        for ( int col = 0; col < sample; ++ col )
        {
            cout << test.at<float>(col, row) << "  ";
            
        }
        cout << endl;
    }
 
    imshow("blocked", temp);
    cvMoveWindow("blocked", temp.size().width, 250);
    

	return test;
}

int Voronoi::blockSize() const {
	return 20;
}


/********** Private Methods ***************************************************/



/********** Local Functions ***************************************************/

bool nonZero(const Mat &matrix) {
    
	for (int row = 0; row < matrix.size().height; row++) {
		for (int col = 0; col < matrix.size().width; col++) {
			if (matrix.at<float>(row, col) == 0) {
				return false;
			}
		}
	}
	return true;
}

Mat flip( Mat matrix) {
    for ( int row = 0; row < matrix.size().height; ++ row )
    {
        for ( int col = 0; col < matrix.size().width; ++ col )
        {
            if(matrix.at<float>(row, col) == 1){
                matrix.at<float>(row, col) =2;
            }
            if(matrix.at<float>(row, col) == 0){
                matrix.at<float>(row, col) =3;
            }
            
        }
    }
    
    
    for ( int row = 0; row < matrix.size().height; ++ row )
    {
        for ( int col = 0; col < matrix.size().width; ++ col )
        {
            if(matrix.at<float>(row, col) == 2){
                matrix.at<float>(row, col) =0;
            }
            if(matrix.at<float>(row, col) == 3){
                matrix.at<float>(row, col) =1;
            }
            
            
        }
    }
	
	return matrix;
}

